{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "using Parameters, AutomotiveDrivingModels, POMDPs, AutoViz, Reel"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "include(\"occluded_crosswalk_env.jl\")\n",
    "include(\"helpers.jl\")\n",
    "include(\"pomdp_types.jl\")\n",
    "include(\"constant_pedestrian.jl\")\n",
    "include(\"generative_model.jl\")\n",
    "include(\"render_helpers.jl\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "rng = MersenneTwister(1)\n",
    "pomdp = OCPOMDP();\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Initial scene with 1 cars\n",
      "Initial ego state: Vehicle(1, VehicleState(VecSE2({5.000, 0.000}, 0.000), Frenet(RoadIndex({1, 0.100000}, {1, 1}), 5.000, 0.000, 0.000), 6.016), VehicleDef(CAR, 4.000, 1.800))\n"
     ]
    }
   ],
   "source": [
    "s0 = initialstate(pomdp, rng)\n",
    "ego = s0[findfirst(s0,1)]\n",
    "println(\"Initial scene with $(length(s0)) cars\")\n",
    "println(\"Initial ego state: $ego\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Try to simulate\n",
    "done = false\n",
    "max_steps = 100\n",
    "step = 0\n",
    "rtot = 0.\n",
    "s = initialstate(pomdp, rng)\n",
    "rec = rec = SceneRecord(max_steps+1, pomdp.ΔT)\n",
    "update!(rec, s)\n",
    "while !done && step < max_steps\n",
    "    sp, o, r = generate_sor(pomdp, s, OCAction(0.0), rng)\n",
    "    done = isterminal(pomdp, sp)\n",
    "    rtot += r\n",
    "    s = sp\n",
    "    update!(rec, sp)\n",
    "    step += 1\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(22,)"
      ]
     },
     "execution_count": 30,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "size(convert_o(Vector{Float64}, o, pomdp))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "overlay = LidarOverlay(pomdp.sensor);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<video autoplay controls><source src=\"files/reel-15647105289333829959.webm?3068405323399265323\" type=\"video/webm\"></video>"
      ],
      "text/plain": [
       "Reel.Frames{MIME{Symbol(\"image/png\")}}(\"C:\\\\Users\\\\Maxime\\\\AppData\\\\Local\\\\Temp\\\\jl_D8C9.tmp\", 0x0000000000000007, 3.5, nothing)"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "fps, duration, render_rec = animate_record(rec, pomdp, overlay)\n",
    "film = roll(render_rec, fps = fps, duration = duration)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "## Measure maximum number of pedestrians"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "2\n",
      "1\n",
      "2\n",
      "1\n",
      "2\n",
      "1\n",
      "2\n",
      "1\n",
      "2\n",
      "1\n",
      "2\n",
      "1\n",
      "2\n",
      "1\n",
      "2\n",
      "1\n",
      "2\n",
      "1\n",
      "2\n",
      "1\n",
      "2\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n",
      "1\n"
     ]
    },
    {
     "ename": "LoadError",
     "evalue": "\u001b[91mAssertionError: veh.id <= pomdp.max_ped + 1\u001b[39m",
     "output_type": "error",
     "traceback": [
      "\u001b[91mAssertionError: veh.id <= pomdp.max_ped + 1\u001b[39m",
      "",
      "Stacktrace:",
      " [1] \u001b[1mgenerate_o\u001b[22m\u001b[22m\u001b[1m(\u001b[22m\u001b[22m::OCPOMDP, ::Records.Frame{Records.Entity{AutomotiveDrivingModels.VehicleState,AutomotiveDrivingModels.VehicleDef,Int64}}, ::OCAction, ::Records.Frame{Records.Entity{AutomotiveDrivingModels.VehicleState,AutomotiveDrivingModels.VehicleDef,Int64}}, ::MersenneTwister\u001b[1m)\u001b[22m\u001b[22m at \u001b[1mC:\\Users\\Maxime\\OneDrive - Leland Stanford Junior University\\Research\\policy-correction\\admenv\\multi_crosswalk\\generative_model.jl:149\u001b[22m\u001b[22m",
      " [2] \u001b[1mgenerate_sor\u001b[22m\u001b[22m\u001b[1m(\u001b[22m\u001b[22m::OCPOMDP, ::Records.Frame{Records.Entity{AutomotiveDrivingModels.VehicleState,AutomotiveDrivingModels.VehicleDef,Int64}}, ::OCAction, ::MersenneTwister\u001b[1m)\u001b[22m\u001b[22m at \u001b[1mC:\\Users\\Maxime\\OneDrive - Leland Stanford Junior University\\Research\\policy-correction\\admenv\\multi_crosswalk\\generative_model.jl:176\u001b[22m\u001b[22m",
      " [3] \u001b[1mmacro expansion\u001b[22m\u001b[22m at \u001b[1m.\\In[7]:12\u001b[22m\u001b[22m [inlined]",
      " [4] \u001b[1manonymous\u001b[22m\u001b[22m at \u001b[1m.\\<missing>:?\u001b[22m\u001b[22m",
      " [5] \u001b[1minclude_string\u001b[22m\u001b[22m\u001b[1m(\u001b[22m\u001b[22m::String, ::String\u001b[1m)\u001b[22m\u001b[22m at \u001b[1m.\\loading.jl:515\u001b[22m\u001b[22m"
     ]
    }
   ],
   "source": [
    "max_ped = 0\n",
    "n_ep = 100\n",
    "for i=1:n_ep\n",
    "    done = false\n",
    "    max_steps = 100\n",
    "    step = 0\n",
    "    rtot = 0.\n",
    "    s = initialstate(pomdp, rng)\n",
    "    rec = rec = SceneRecord(max_steps+1, pomdp.ΔT)\n",
    "    update!(rec, s)\n",
    "    while !done && step < max_steps\n",
    "        sp, o, r = generate_sor(pomdp, s, OCAction(0.0), rng)\n",
    "        if length(sp) > max_ped\n",
    "            max_ped = length(sp)\n",
    "        end\n",
    "        for veh in sp\n",
    "            println(veh.id)\n",
    "        end\n",
    "        done = isterminal(pomdp, sp)\n",
    "        rtot += r\n",
    "        s = sp\n",
    "        update!(rec, sp)\n",
    "        step += 1\n",
    "    end\n",
    "end\n",
    "max_ped -= 1\n",
    "println(\"Maximum number of pedestrians observed over $n_ep: $max_ped\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Julia 0.6.0",
   "language": "julia",
   "name": "julia-0.6"
  },
  "language_info": {
   "file_extension": ".jl",
   "mimetype": "application/julia",
   "name": "julia",
   "version": "0.6.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
